package RVO;

import javax.swing.*;
import java.awt.*;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.util.ArrayList;
import java.util.Random;

//创建指定agent往指定目标移动, 用于观测算法是否正确
class ORCAVisualAuto extends JPanel implements ActionListener {
    private Timer timer;
    private int frameCount;
    private Vector2 []goals = new Vector2[100];
    private Random rand;
    private Simulator simulator;

    public ORCAVisualAuto() {
        simulator = new Simulator();
        frameCount = 0;
        rand = new Random();

        timer = new Timer(33, this); // 更快的更新频率
        timer.start();

        setPreferredSize(new Dimension(500, 700));
        setBackground(Color.WHITE);

        /* Specify the global time step of the simulation. */
        simulator.setTimeStep(2f);

        /*
         * Specify the default parameters for agents that are subsequently
         * added.
         */
        simulator.setAgentDefaults(50.0f, 10, 25.0f, 5.0f, 10f, 2.0f, new Vector2(0.0f, 0.0f));
        /*
         * Add agents, specifying their start position, and store their
         * goals on the opposite side of the environment.
         */
        int numAgents = 30;

        for (int i = 0; i < numAgents; i++) {
            double angle = 2 * Math.PI * i / numAgents;
            double radius = 200;

            Vector2 position = new Vector2(
                    (float) (250 + radius * Math.cos(angle)),
                    (float) (300 + radius * Math.sin(angle))
            );
            simulator.addAgent(position);
            simulator.agents_.get(i).name = "" + i;
            simulator.agents_.get(i).timeStep_ = i == 0 ? 4.f : i == 1 ? 0.0f : 2.f;
            simulator.agents_.get(i).radius_ = rand.nextInt(6) + 8;
            simulator.agents_.get(i).color = new Color(rand.nextInt(200) + 55, rand.nextInt(200) + 55, rand.nextInt(200) + 55);

            // 目标在对面的位置
            Vector2 goal = new Vector2(
                    50 + rand.nextInt(400),
                    50 + rand.nextInt(500)
            );
            goals[i] = goal;
        }
    }

    @Override
    protected void paintComponent(Graphics g) {
        super.paintComponent(g);
        Graphics2D g2d = (Graphics2D) g;
        g2d.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);

        // 绘制信息
        g2d.setColor(Color.BLACK);
        g2d.setFont(new Font("Arial", Font.PLAIN, 14));
        g2d.drawString("ORCA避障算法演示 - ", 10, 20);
        g2d.drawString("帧数: " + frameCount, 10, 40);

        for (int i = 0; i < simulator.getNumAgents(); ++ i) {
            Agent agent = simulator.agents_.get(i);
            g2d.setColor(agent.color);
            g2d.fillOval(
                    (int) (agent.position_.x_ - agent.radius_),
                    (int) (agent.position_.y_ - agent.radius_),
                    (int) (agent.radius_ * 2),
                    (int) (agent.radius_ * 2)
            );

            Vector2 goal = goals[i];
            g2d.setColor(agent.color.darker());
            g2d.fillOval(
                    (int) (goal.x_ - 4),
                    (int) (goal.y_ - 4),
                    8, 8
            );

            // 绘制边框
            g2d.setColor(Color.BLACK);
            g2d.drawOval(
                    (int) (agent.position_.x_ - agent.radius_),
                    (int) (agent.position_.y_ - agent.radius_),
                    (int) (agent.radius_ * 2),
                    (int) (agent.radius_ * 2)
            );

            // 绘制编号
            g2d.setColor(Color.WHITE);
            g2d.setFont(new Font("Arial", Font.BOLD, 12));
            g2d.drawString(String.valueOf(i + 1),
                    (int) agent.position_.x_ - 4,
                    (int) agent.position_.y_ + 4);

            // 绘制速度向量
            g2d.setColor(Color.RED);
            Vector2 velocityEnd = Vector2.add(agent.position_, Vector2.multiply(agent.velocity_, 20f));
            g2d.drawLine(
                    (int) agent.position_.x_,
                    (int) agent.position_.y_,
                    (int) velocityEnd.x_,
                    (int) velocityEnd.y_
            );
        }
    }

    public void setPreferredVelocities() {
        /*
         * Set the preferred velocity to be a vector of unit magnitude
         * (speed) in the direction of the goal.
         */
        for (int i = 0; i < simulator.getNumAgents(); ++i) {
            int agentNo = simulator.agents_.get(i).id_;
            Vector2 goalVector = Vector2.subtract(goals[i], simulator.getAgentPosition(agentNo));
            if (goalVector.x_ * goalVector.x_ + goalVector.y_ * goalVector.y_ > 1.0f) {
                goalVector = Vector2.divide(goalVector, ((float) Math.sqrt(goalVector.x_ * goalVector.x_
                        + goalVector.y_ * goalVector.y_)));
            }

            //System.out.printf("%s 目标%s  移动向量[%f,%f]\n", Simulator.Instance.getAgentPosition(i),
                    //mousePoint, goalVector.x_, goalVector.y_);
            simulator.setAgentPrefVelocity(agentNo, goalVector);
        }
    }

    @Override
    public void actionPerformed(ActionEvent e) {
        frameCount++;
        if (frameCount == 500) {
            simulator.delAgent(1);
        }
        System.out.printf("执行%d帧\n", frameCount);
        setPreferredVelocities();
        simulator.doStep();
        repaint();

        for (int i = 0; i < simulator.agents_.size(); ++ i) {
            Agent agent = simulator.agents_.get(i);
            Vector2 goal = goals[i];
            if (RVOMath.absSq(Vector2.subtract(agent.position_, goal)) < 20) {
                goals[i] = new Vector2(
                        50 + rand.nextInt(400),
                        50 + rand.nextInt(500)
                );
            }
        }
    }
}

// 主类
public class Demo3 {
    public static void main(String[] args) {
        SwingUtilities.invokeLater(() -> {
            JFrame frame = new JFrame("ORCA避障算法演示");
            frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
            frame.add(new ORCAVisualAuto());
            frame.pack();
            frame.setLocationRelativeTo(null);
            frame.setVisible(true);
        });
    }
}